/*
 * ekf_uwb.hpp
 *
 *  Created on: 2023年6月25日
 *      Author: CJP
 */

#ifndef MODULES_EKF_UWB_YAW_EKF_UWB_YAW_HPP_
#define MODULES_EKF_UWB_YAW_EKF_UWB_YAW_HPP_

#include "system/system.hpp"
#include "userlib/userlib.hpp"


#ifdef __cplusplus

#include <Eigen>

using namespace std;
using namespace Eigen;

#define yaw_cali_window 1000
#define yaw_cali_window_depart 10

class EKFUWB
{
public:
	EKFUWB(bool isfirstekf = true)
	{
		this -> isFirstEKF = isfirstekf;
	}
	EKFUWB(char *n) : name(n){}
	~EKFUWB(){}
	void EkfUwb_Init();
	void EkfUwb_acc();
	void EkfUwb_getJacobin();
	void EkfUwb_Update();
	void EkfUwb_Ins();
	void EkfUwb_YawCali();
	bool EkfUwb_isYawCali();


private:
	char *name;
	sTIM Tim;
	bool Update;
	bool isFirstIns;
	bool isFirstEKF;
	bool isYawCali;
	uint32_t startTimer;
	uint32_t stopTimer;
	uint32_t  executionTime_us;
	uint32_t startTimerLast;
	uint32_t cycleTime_us;

	uint32_t startTimer1;
	uint32_t stopTimer1;
	uint32_t  executionTime_us1;
	uint32_t startTimerLast1;
	uint32_t cycleTime_us1;

	uint32_t startTimer2;
	uint32_t stopTimer2;
	uint32_t  executionTime_us2;

	Quaternion<float> q;


	//矩阵运算变量定义（EKF）

	Matrix<float,11,1> X_p;		//状态预测
	Matrix<float,11,1> X_k;		//状态更新
	Matrix<float,6,1> y;		//测量值
	Matrix<float,6,1> y_k;      //模型输出
	Vector3f temp_xold_8_10;
	Vector3f temp_xpre_3_5;
	Vector3f temp_xold_3_5;
	Vector3f temp_yk_3_5;

	Matrix<float,11,11> Jacobin_A;	//转移矩阵雅可比
	Matrix<float,6,11> Jacobin_H;	//输出矩阵雅可比
	Matrix<float,11,11> Jacobin_W;	//过程噪声雅可比
	Matrix<float,6,6> Jacobin_V;	//测量噪声雅可比

	Matrix<float,11,11> P_p;	//协方差矩阵预测
	Matrix<float,11,11> P_k;	//协方差矩阵更新
	Matrix<float,11,6> K;       //卡尔曼增益
	Matrix<float,6,6> R;		//测量噪声
	Matrix<float,11,11> Q;		//过程噪声

	//-----------------yaw_cali 变量
	Matrix<float,3,yaw_cali_window / yaw_cali_window_depart> source_xyz; //ins坐标
	Matrix<float,3,yaw_cali_window / yaw_cali_window_depart> target_xyz; //uwb坐标
	Matrix<float,3,3> A;
	Matrix<float,3,3> U;
	Matrix<float,3,3> V;
	Matrix<float,3,3> Rotation;
	Vector3f source_temp;
	Vector3f target_temp;
	float lamda;
	float lamda_Ttemp;
	float lamda_temp;
	float yaw_cali;

	Matrix<float,3,3> Cb2n;	//机体坐标系到NED的旋转矩阵

	Vector3f a_k;			//机体坐标系下的加速度
	Vector3f accUWB;
	float gyro_z;           //机体系下的偏航角速度
	float gyrozUWB;
	Matrix3f Rz;			//NED转UWB坐标系旋转矩阵
	Matrix<float,4,1> u_k;			//
	float dt;               //
	float yaw_mag_record;
	float yaw;

	int YawCaliCnt;

	sensor_acc_msg acc;
	sensor_gyro_filter_msg gyro;
	uwb_pos_msg uwbPos;
	flow_msg flow;
	height_msg height;
	ekf_uwb_pos_vel_msg ekf_uwb_pos_vel;
	ahrs_euler_msg ahrsEuler;

};

#endif

#endif /* MODULES_EKF_UWB_YAW_EKF_UWB_YAW_HPP_ */
